#include "myfuncs.h"

using namespace cv;
using namespace std;

void interceptDepthMap(const Mat& src, Mat& dst, int position, uchar invalid)
{
    CV_Assert(src.type() == CV_16U);
    position = MAX(position, 0);
    position = MIN(position, 8);

    int posShift = 8 - position;

    dst = Mat::zeros(src.size(), CV_8UC1);
    int sumPix = src.rows * src.cols;
    uchar* ptrSrc0 = src.data;
    uchar* ptrSrc1 = src.data + 1;
    uchar* ptrDst = dst.data;
    for (int i = 0; i < sumPix; i++) {
        if ((*ptrSrc1 >> position) > 0)
            *ptrDst = invalid;
        else
            *ptrDst = (*ptrSrc0 >> position) + (*ptrSrc1 << posShift);
        ptrSrc0 += 2;
        ptrSrc1 += 2;
        ptrDst++;
    }
}

void colorDisparity(const cv::Mat& msrc, cv::Mat& mdst, int nmax)
{
    double ninterval = 300. / nmax;
    int alpha = nmax * 0.8;
    mdst.create(msrc.size(), CV_8UC3);
    int ar[] = { 0, 119, 119, 110, 110, 102, 102, 93, 93, 85, 85, 76, 76, 68,
        68, 59, 59, 51, 51, 42, 42, 34, 34, 25, 25, 17, 17, 8, 8, 0, 0, 0,
        0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
        0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
        0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
        0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
        0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
        0, 0, 0, 0, 0, 0, 0, 0, 0, 8, 8, 17, 17, 25, 25, 34, 34, 42, 42,
        51, 51, 59, 59, 68, 68, 76, 76, 85, 85, 93, 93, 102, 102, 110,
        110, 119, 119, 127, 127, 135, 135, 144, 144, 152, 152, 161, 161,
        169, 169, 178, 178, 186, 186, 195, 195, 203, 203, 212, 212, 220,
        220, 229, 229, 237, 237, 246, 246, 254, 254, 254, 254, 254, 254,
        254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254,
        254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254,
        254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254,
        254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254,
        254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254,
        254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254,
        254, 254, 254, 254, 254, 254, 254, 254 };
    int ag[] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
        0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 8, 8, 17, 17, 25, 25, 34, 34, 42,
        42, 51, 51, 59, 59, 68, 68, 76, 76, 85, 85, 93, 93, 102, 102, 110,
        110, 119, 119, 127, 127, 135, 135, 144, 144, 152, 152, 161, 161,
        169, 169, 178, 178, 186, 186, 195, 195, 203, 203, 212, 212, 220,
        220, 229, 229, 237, 237, 246, 246, 254, 254, 254, 254, 254, 254,
        254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254,
        254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254,
        254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254,
        254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254,
        254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254,
        254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254,
        254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254,
        254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254,
        254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 246,
        246, 237, 237, 229, 229, 220, 220, 212, 212, 203, 203, 195, 195,
        186, 186, 178, 178, 169, 169, 161, 161, 152, 152, 144, 144, 135,
        135, 127, 127, 119, 119, 110, 110, 102, 102, 93, 93, 85, 85, 76,
        76, 68, 68, 59, 59, 51, 51, 42, 42, 34, 34, 25, 25, 17, 17, 8, 8,
        0, 0, 0, 8, 8, 17, 17, 25, 25, 34, 34, 42, 42, 51, 51, 59, 59, 68,
        68, 76, 76, 85, 85, 93, 93, 102, 102, 110, 110, 119, 119, 127 };
    int ab[] = { 0, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254,
        254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254,
        254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254,
        254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254,
        254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254,
        254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254,
        254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254,
        246, 246, 237, 237, 229, 229, 220, 220, 212, 212, 203, 203, 195,
        195, 186, 186, 178, 178, 169, 169, 161, 161, 152, 152, 144, 144,
        135, 135, 127, 127, 119, 119, 110, 110, 102, 102, 93, 93, 85, 85,
        76, 76, 68, 68, 59, 59, 51, 51, 42, 42, 34, 34, 25, 25, 17, 17, 8,
        8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
        0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
        0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
        0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
        0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
        0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
        0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };

    for (int _row = 0; _row < msrc.rows; _row++) {
        for (int _col = 0; _col < msrc.cols; _col++) {
            int r, g, b;
            int disp = msrc.at<uchar>(_row, _col);
            if (disp > nmax) {
                r = ar[300];
                g = ag[300];
                b = ab[300];
            } else {
                int idx = (int)disp * ninterval;
                r = ar[idx];
                g = ag[idx];
                b = ab[idx];
            }
            mdst.at<cv::Vec3b>(_row, _col) = cv::Vec3b(b, g, r);
        }
    }
}
